namespace NBody.Simulation
{
    using System;
            
    public class Hermite : Base
    {
        public Hermite()
        {
        }
                
        protected override void Initialize(out double epot, out double coll_time)
        {
            for (int i = 0; i < system.NumParts; i++)
                system[i] = new HermiteParticle(system[i]);
            
            calc_acc_jerk(out epot, out coll_time);
        }     
        
        protected override void Iterate(double dt, out double epot, out double coll_time)
        {
            NBodySystem s_old = new NBodySystem(system.NumParts, system.Time);
            for (int i = 0; i < system.NumParts; i++)
            {
                HermiteParticle pn = (HermiteParticle)system[i];
                HermiteParticle p = new HermiteParticle(pn);
                p.SetAcceleration(pn.Ax, pn.Ay, pn.Az);
                p.SetJerk(pn.JerkX, pn.JerkY, pn.JerkZ);
                s_old.AddParticle(p);
            }            
            
            predict_step(dt);
            calc_acc_jerk(out epot, out coll_time);
            correct_step(s_old, dt);
        }
        
        private void predict_step(double dt)
        {
            foreach (HermiteParticle p in system)
            {
                double x = p.X + p.Vx * dt + p.Ax * dt * dt / 2.0 + p.JerkX * dt * dt * dt / 6.0;
                double y = p.Y + p.Vy * dt + p.Ay * dt * dt / 2.0 + p.JerkY * dt * dt * dt / 6.0;
                double z = p.Z + p.Vz * dt + p.Az * dt * dt / 2.0 + p.JerkZ * dt * dt * dt / 6.0;
                
                double vx = p.Vx + p.Ax * dt + p.JerkX * dt * dt / 2.0;
                double vy = p.Vy + p.Ay * dt + p.JerkY * dt * dt / 2.0;
                double vz = p.Vz + p.Az * dt + p.JerkZ * dt * dt / 2.0;
                
                p.SetPosition(x, y, z);
                p.SetVelocity(vx, vy, vz);
            }
        }
        
        private void correct_step(NBodySystem old, double dt)
        {
            for (int i = 0; i < system.NumParts; i++)
            {
                HermiteParticle p = system[i] as HermiteParticle;
                HermiteParticle po = old[i] as HermiteParticle;
                
                double vx = po.Vx + (po.Ax + p.Ax) * dt / 2.0 + (po.JerkX - p.JerkX) * dt * dt / 12.0;
                double vy = po.Vy + (po.Ay + p.Ay) * dt / 2.0 + (po.JerkY - p.JerkY) * dt * dt / 12.0;
                double vz = po.Vz + (po.Az + p.Az) * dt / 2.0 + (po.JerkZ - p.JerkZ) * dt * dt / 12.0;
                
                double x = po.X + (po.Vx + p.Vx) * dt / 2.0 + (po.Ax - p.Ax) * dt * dt / 12.0;
                double y = po.Y + (po.Vy + p.Vy) * dt / 2.0 + (po.Ay - p.Ay) * dt * dt / 12.0;
                double z = po.Z + (po.Vz + p.Vz) * dt / 2.0 + (po.Az - p.Az) * dt * dt / 12.0;
                
                p.SetPosition(x, y, z);
                p.SetVelocity(vx, vy, vz);
            }
        }
        
        private void calc_acc_jerk(out double epot, out double coll_time)
        {
            foreach (HermiteParticle p in system)
            {
                p.SetAcceleration(0.0, 0.0, 0.0);
                p.SetJerk(0.0, 0.0, 0.0);
            }
            
            epot = 0.0;
            
            double coll_time_q = Double.PositiveInfinity;
            double coll_est_q;
            double[] rji = {0.0, 0.0, 0.0};
            double[] vji = {0.0, 0.0, 0.0};
            double[] da = {0.0, 0.0, 0.0};
            double[] dj = {0.0, 0.0, 0.0};
            for (int i = 0; i < system.NumParts; i++)
            {
                HermiteParticle pi = system[i] as HermiteParticle;
                for (int j = i + 1; j < system.NumParts; j++)
                {
                    HermiteParticle pj = system[j] as HermiteParticle;
                    for (int k = 0; k < 3 ; k++)
                    {
                        rji[k] = pj.Position(k) - pi.Position(k);
                        vji[k] = pj.Velocity(k) - pi.Velocity(k);
                    }
                    
                    double r2 = 0.0, v2 = 0.0, rv_r2 = 0.0;
                    for (int k = 0; k < 3; k++)
                    {
                        r2 += rji[k] * rji[k];
                        v2 += vji[k] * vji[k];
                        rv_r2 += rji[k] * vji[k];
                    }
                    rv_r2 /= r2;
                    double r = Math.Sqrt(r2);
                    double r3 = r * r2;
                    
                    epot -= pi.Mass * pj.Mass / r;
                    
                    for (int k = 0; k < 3; k++)
                    {
                        da[k] = rji[k] / r3;
                        dj[k] = (vji[k] - 3.0 * rv_r2 * rji[k]) / r3;
                    }
                    double ax = pi.Ax + pj.Mass * da[0];
                    double ay = pi.Ay + pj.Mass * da[1];
                    double az = pi.Az + pj.Mass * da[2];
                    pi.SetAcceleration(ax, ay, az);
                    ax = pj.Ax - pi.Mass * da[0];
                    ay = pj.Ay - pi.Mass * da[1];
                    az = pj.Az - pi.Mass * da[2];
                    pj.SetAcceleration(ax, ay, az);
                    
                    double jx = pi.JerkX + pj.Mass * dj[0];
                    double jy = pi.JerkY + pj.Mass * dj[1];
                    double jz = pi.JerkZ + pj.Mass * dj[2];
                    pi.SetJerk(jx, jy, jz);
                    jx = pj.JerkX - pi.Mass * dj[0];
                    jy = pj.JerkY - pi.Mass * dj[1];
                    jz = pj.JerkZ - pi.Mass * dj[2];
                    pj.SetJerk(jx, jy, jz);
                    
                    coll_est_q = (r2 * r2) / (v2 * v2);
                    if (coll_time_q > coll_est_q)
                        coll_time_q = coll_est_q;
                    
                    double da2 = 0.0;
                    for (int k = 0; k < 3; k++)
                        da2 += da[k] * da[k];
                    double mij = pi.Mass + pj.Mass;
                    da2 *= mij * mij;
                    
                    coll_est_q = r2 / da2;
                    if (coll_time_q > coll_est_q)
                        coll_time_q = coll_est_q;
                }
            }
            coll_time = Math.Sqrt(Math.Sqrt(coll_time_q));
        }
    }
}
